#include <transforms.h>

#include <debug.h>
#include <unittest.h>

/**
 * DESC: Test transformations
 * IMP: HIGH
 */

#define TEST_ANGLE M_PI / 6

int main( int argc, char *argv[] )
{
  debug_level = DEBUG_INFO;

  char *str, *str2, *str3;
  matrix4x4_t *m;
  double s, c;

  vector3_t vi, vj, vk, v;
  vector3_set( &vi, 1, 0, 0 );
  vector3_set( &vj, 0, 1, 0 );
  vector3_set( &vk, 0, 0, 1 );


  s = sin( TEST_ANGLE );
  c = cos( TEST_ANGLE );
  m = rotate_z( TEST_ANGLE );
  str = matrix4x4_str( *m );
  info( "rotate_z( %lf ) =\n%s\n", TEST_ANGLE, str );
  free( str );
  assert_equal_double( m->data[ 0 ][ 0 ],  c, DOUBLE_PRECISION );
  assert_equal_double( m->data[ 0 ][ 1 ], -s, DOUBLE_PRECISION );
  assert_equal_double( m->data[ 0 ][ 2 ],  0, DOUBLE_PRECISION );
  assert_equal_double( m->data[ 0 ][ 3 ],  0, DOUBLE_PRECISION );
  assert_equal_double( m->data[ 1 ][ 0 ],  s, DOUBLE_PRECISION );
  assert_equal_double( m->data[ 1 ][ 1 ],  c, DOUBLE_PRECISION );
  assert_equal_double( m->data[ 1 ][ 2 ],  0, DOUBLE_PRECISION );
  assert_equal_double( m->data[ 1 ][ 3 ],  0, DOUBLE_PRECISION );
  assert_equal_double( m->data[ 2 ][ 0 ],  0, DOUBLE_PRECISION );
  assert_equal_double( m->data[ 2 ][ 1 ],  0, DOUBLE_PRECISION );
  assert_equal_double( m->data[ 2 ][ 2 ],  1, DOUBLE_PRECISION );
  assert_equal_double( m->data[ 2 ][ 3 ],  0, DOUBLE_PRECISION );
  assert_equal_double( m->data[ 3 ][ 0 ],  0, DOUBLE_PRECISION );
  assert_equal_double( m->data[ 3 ][ 1 ],  0, DOUBLE_PRECISION );
  assert_equal_double( m->data[ 3 ][ 2 ],  0, DOUBLE_PRECISION );
  assert_equal_double( m->data[ 3 ][ 3 ],  1, DOUBLE_PRECISION );
  matrix4x4_free( m );

  m = rotate_x( TEST_ANGLE );
  str = matrix4x4_str( *m );
  info( "rotate_x( %lf ) =\n%s\n", TEST_ANGLE, str );
  free( str );
  assert_equal_double( m->data[ 0 ][ 0 ],  1, DOUBLE_PRECISION );
  assert_equal_double( m->data[ 0 ][ 1 ],  0, DOUBLE_PRECISION );
  assert_equal_double( m->data[ 0 ][ 2 ],  0, DOUBLE_PRECISION );
  assert_equal_double( m->data[ 0 ][ 3 ],  0, DOUBLE_PRECISION );
  assert_equal_double( m->data[ 1 ][ 0 ],  0, DOUBLE_PRECISION );
  assert_equal_double( m->data[ 1 ][ 1 ],  c, DOUBLE_PRECISION );
  assert_equal_double( m->data[ 1 ][ 2 ], -s, DOUBLE_PRECISION );
  assert_equal_double( m->data[ 1 ][ 3 ],  0, DOUBLE_PRECISION );
  assert_equal_double( m->data[ 2 ][ 0 ],  0, DOUBLE_PRECISION );
  assert_equal_double( m->data[ 2 ][ 1 ],  s, DOUBLE_PRECISION );
  assert_equal_double( m->data[ 2 ][ 2 ],  c, DOUBLE_PRECISION );
  assert_equal_double( m->data[ 2 ][ 3 ],  0, DOUBLE_PRECISION );
  assert_equal_double( m->data[ 3 ][ 0 ],  0, DOUBLE_PRECISION );
  assert_equal_double( m->data[ 3 ][ 1 ],  0, DOUBLE_PRECISION );
  assert_equal_double( m->data[ 3 ][ 2 ],  0, DOUBLE_PRECISION );
  assert_equal_double( m->data[ 3 ][ 3 ],  1, DOUBLE_PRECISION );
  matrix4x4_free( m );


  m = rotate_y( TEST_ANGLE );
  str = matrix4x4_str( *m );
  info( "rotate_y( %lf ) =\n%s\n", TEST_ANGLE, str );
  free( str );
  assert_equal_double( m->data[ 0 ][ 0 ],  c, DOUBLE_PRECISION );
  assert_equal_double( m->data[ 0 ][ 1 ],  0, DOUBLE_PRECISION );
  assert_equal_double( m->data[ 0 ][ 2 ],  s, DOUBLE_PRECISION );
  assert_equal_double( m->data[ 0 ][ 3 ],  0, DOUBLE_PRECISION );
  assert_equal_double( m->data[ 1 ][ 0 ],  0, DOUBLE_PRECISION );
  assert_equal_double( m->data[ 1 ][ 1 ],  1, DOUBLE_PRECISION );
  assert_equal_double( m->data[ 1 ][ 2 ],  0, DOUBLE_PRECISION );
  assert_equal_double( m->data[ 1 ][ 3 ],  0, DOUBLE_PRECISION );
  assert_equal_double( m->data[ 2 ][ 0 ], -s, DOUBLE_PRECISION );
  assert_equal_double( m->data[ 2 ][ 1 ],  0, DOUBLE_PRECISION );
  assert_equal_double( m->data[ 2 ][ 2 ],  c, DOUBLE_PRECISION );
  assert_equal_double( m->data[ 2 ][ 3 ],  0, DOUBLE_PRECISION );
  assert_equal_double( m->data[ 3 ][ 0 ],  0, DOUBLE_PRECISION );
  assert_equal_double( m->data[ 3 ][ 1 ],  0, DOUBLE_PRECISION );
  assert_equal_double( m->data[ 3 ][ 2 ],  0, DOUBLE_PRECISION );
  assert_equal_double( m->data[ 3 ][ 3 ],  1, DOUBLE_PRECISION );
  matrix4x4_free( m );

  vector3_set( &v, 11, 33, 55 );
  m    = translate( v );
  str  = matrix4x4_str( *m );
  str2 = vector3_str( v );
  info( "translate( %s ) =\n%s\n", str2, str );
  free( str );
  free( str2 );
  assert_equal_double( m->data[ 0 ][ 0 ],  1, DOUBLE_PRECISION );
  assert_equal_double( m->data[ 0 ][ 1 ],  0, DOUBLE_PRECISION );
  assert_equal_double( m->data[ 0 ][ 2 ],  0, DOUBLE_PRECISION );
  assert_equal_double( m->data[ 0 ][ 3 ], 11, DOUBLE_PRECISION );
  assert_equal_double( m->data[ 1 ][ 0 ],  0, DOUBLE_PRECISION );
  assert_equal_double( m->data[ 1 ][ 1 ],  1, DOUBLE_PRECISION );
  assert_equal_double( m->data[ 1 ][ 2 ],  0, DOUBLE_PRECISION );
  assert_equal_double( m->data[ 1 ][ 3 ], 33, DOUBLE_PRECISION );
  assert_equal_double( m->data[ 2 ][ 0 ],  0, DOUBLE_PRECISION );
  assert_equal_double( m->data[ 2 ][ 1 ],  0, DOUBLE_PRECISION );
  assert_equal_double( m->data[ 2 ][ 2 ],  1, DOUBLE_PRECISION );
  assert_equal_double( m->data[ 2 ][ 3 ], 55, DOUBLE_PRECISION );
  assert_equal_double( m->data[ 3 ][ 0 ],  0, DOUBLE_PRECISION );
  assert_equal_double( m->data[ 3 ][ 1 ],  0, DOUBLE_PRECISION );
  assert_equal_double( m->data[ 3 ][ 2 ],  0, DOUBLE_PRECISION );
  assert_equal_double( m->data[ 3 ][ 3 ],  1, DOUBLE_PRECISION );
  matrix4x4_free( m );

  vector3_set( &v, 11, 33, 55 );
  m    = inv_translate( v );
  str  = matrix4x4_str( *m );
  str2 = vector3_str( v );
  info( "inv_translate( %s ) =\n%s\n", str2, str );
  free( str );
  free( str2 );
  assert_equal_double( m->data[ 0 ][ 0 ],   1, DOUBLE_PRECISION );
  assert_equal_double( m->data[ 0 ][ 1 ],   0, DOUBLE_PRECISION );
  assert_equal_double( m->data[ 0 ][ 2 ],   0, DOUBLE_PRECISION );
  assert_equal_double( m->data[ 0 ][ 3 ], -11, DOUBLE_PRECISION );
  assert_equal_double( m->data[ 1 ][ 0 ],   0, DOUBLE_PRECISION );
  assert_equal_double( m->data[ 1 ][ 1 ],   1, DOUBLE_PRECISION );
  assert_equal_double( m->data[ 1 ][ 2 ],   0, DOUBLE_PRECISION );
  assert_equal_double( m->data[ 1 ][ 3 ], -33, DOUBLE_PRECISION );
  assert_equal_double( m->data[ 2 ][ 0 ],   0, DOUBLE_PRECISION );
  assert_equal_double( m->data[ 2 ][ 1 ],   0, DOUBLE_PRECISION );
  assert_equal_double( m->data[ 2 ][ 2 ],   1, DOUBLE_PRECISION );
  assert_equal_double( m->data[ 2 ][ 3 ], -55, DOUBLE_PRECISION );
  assert_equal_double( m->data[ 3 ][ 0 ],   0, DOUBLE_PRECISION );
  assert_equal_double( m->data[ 3 ][ 1 ],   0, DOUBLE_PRECISION );
  assert_equal_double( m->data[ 3 ][ 2 ],   0, DOUBLE_PRECISION );
  assert_equal_double( m->data[ 3 ][ 3 ],   1, DOUBLE_PRECISION );
  matrix4x4_free( m );

  vector3_set( &v, 11, 33, 55 );
  m    = scale( v );
  str  = matrix4x4_str( *m );
  str2 = vector3_str( v );
  info( "scale( %s ) =\n%s\n", str2, str );
  free( str );
  free( str2 );
  assert_equal_double( m->data[ 0 ][ 0 ], 11, DOUBLE_PRECISION );
  assert_equal_double( m->data[ 0 ][ 1 ],  0, DOUBLE_PRECISION );
  assert_equal_double( m->data[ 0 ][ 2 ],  0, DOUBLE_PRECISION );
  assert_equal_double( m->data[ 0 ][ 3 ],  0, DOUBLE_PRECISION );
  assert_equal_double( m->data[ 1 ][ 0 ],  0, DOUBLE_PRECISION );
  assert_equal_double( m->data[ 1 ][ 1 ], 33, DOUBLE_PRECISION );
  assert_equal_double( m->data[ 1 ][ 2 ],  0, DOUBLE_PRECISION );
  assert_equal_double( m->data[ 1 ][ 3 ],  0, DOUBLE_PRECISION );
  assert_equal_double( m->data[ 2 ][ 0 ],  0, DOUBLE_PRECISION );
  assert_equal_double( m->data[ 2 ][ 1 ],  0, DOUBLE_PRECISION );
  assert_equal_double( m->data[ 2 ][ 2 ], 55, DOUBLE_PRECISION );
  assert_equal_double( m->data[ 2 ][ 3 ],  0, DOUBLE_PRECISION );
  assert_equal_double( m->data[ 3 ][ 0 ],  0, DOUBLE_PRECISION );
  assert_equal_double( m->data[ 3 ][ 1 ],  0, DOUBLE_PRECISION );
  assert_equal_double( m->data[ 3 ][ 2 ],  0, DOUBLE_PRECISION );
  assert_equal_double( m->data[ 3 ][ 3 ],  1, DOUBLE_PRECISION );
  matrix4x4_free( m );

  vector3_set( &v, 11, 33, 55 );
  m    = inv_scale( v );
  str  = matrix4x4_str( *m );
  str2 = vector3_str( v );
  info( "inv_scale( %s ) =\n%s\n", str2, str );
  free( str );
  free( str2 );
  assert_equal_double( m->data[ 0 ][ 0 ], 1./11, DOUBLE_PRECISION );
  assert_equal_double( m->data[ 0 ][ 1 ],     0, DOUBLE_PRECISION );
  assert_equal_double( m->data[ 0 ][ 2 ],     0, DOUBLE_PRECISION );
  assert_equal_double( m->data[ 0 ][ 3 ],     0, DOUBLE_PRECISION );
  assert_equal_double( m->data[ 1 ][ 0 ],     0, DOUBLE_PRECISION );
  assert_equal_double( m->data[ 1 ][ 1 ], 1./33, DOUBLE_PRECISION );
  assert_equal_double( m->data[ 1 ][ 2 ],     0, DOUBLE_PRECISION );
  assert_equal_double( m->data[ 1 ][ 3 ],     0, DOUBLE_PRECISION );
  assert_equal_double( m->data[ 2 ][ 0 ],     0, DOUBLE_PRECISION );
  assert_equal_double( m->data[ 2 ][ 1 ],     0, DOUBLE_PRECISION );
  assert_equal_double( m->data[ 2 ][ 2 ], 1./55, DOUBLE_PRECISION );
  assert_equal_double( m->data[ 2 ][ 3 ],     0, DOUBLE_PRECISION );
  assert_equal_double( m->data[ 3 ][ 0 ],     0, DOUBLE_PRECISION );
  assert_equal_double( m->data[ 3 ][ 1 ],     0, DOUBLE_PRECISION );
  assert_equal_double( m->data[ 3 ][ 2 ],     0, DOUBLE_PRECISION );
  assert_equal_double( m->data[ 3 ][ 3 ],     1, DOUBLE_PRECISION );
  matrix4x4_free( m );

  m = rotate_z( M_PI_2 );

  multiply_matrix4x4_vector3( *m, vi, &v );
  assert_equal_double( v.data[ 0 ], vj.data[ 0 ], DOUBLE_PRECISION );
  assert_equal_double( v.data[ 1 ], vj.data[ 1 ], DOUBLE_PRECISION );
  assert_equal_double( v.data[ 2 ], vj.data[ 2 ], DOUBLE_PRECISION );

  multiply_matrix4x4_vector3( *m, vj, &v );
  assert_equal_double( v.data[ 0 ], -vi.data[ 0 ], DOUBLE_PRECISION );
  assert_equal_double( v.data[ 1 ], -vi.data[ 1 ], DOUBLE_PRECISION );
  assert_equal_double( v.data[ 2 ], -vi.data[ 2 ], DOUBLE_PRECISION );

  matrix4x4_free( m );

  m = rotate_y( M_PI_2 );

  multiply_matrix4x4_vector3( *m, vi, &v );
  assert_equal_double( v.data[ 0 ], -vk.data[ 0 ], DOUBLE_PRECISION );
  assert_equal_double( v.data[ 1 ], -vk.data[ 1 ], DOUBLE_PRECISION );
  assert_equal_double( v.data[ 2 ], -vk.data[ 2 ], DOUBLE_PRECISION );

  multiply_matrix4x4_vector3( *m, vk, &v );
  assert_equal_double( v.data[ 0 ], vi.data[ 0 ], DOUBLE_PRECISION );
  assert_equal_double( v.data[ 1 ], vi.data[ 1 ], DOUBLE_PRECISION );
  assert_equal_double( v.data[ 2 ], vi.data[ 2 ], DOUBLE_PRECISION );

  matrix4x4_free( m );

  m = rotate_x( M_PI_2 );

  multiply_matrix4x4_vector3( *m, vj, &v );
  assert_equal_double( v.data[ 0 ], vk.data[ 0 ], DOUBLE_PRECISION );
  assert_equal_double( v.data[ 1 ], vk.data[ 1 ], DOUBLE_PRECISION );
  assert_equal_double( v.data[ 2 ], vk.data[ 2 ], DOUBLE_PRECISION );

  multiply_matrix4x4_vector3( *m, vk, &v );
  assert_equal_double( v.data[ 0 ], -vj.data[ 0 ], DOUBLE_PRECISION );
  assert_equal_double( v.data[ 1 ], -vj.data[ 1 ], DOUBLE_PRECISION );
  assert_equal_double( v.data[ 2 ], -vj.data[ 2 ], DOUBLE_PRECISION );

  matrix4x4_free( m );

  matrix4x4_t *pipeline[] = {
    rotate_x( TEST_ANGLE ),
    rotate_y( TEST_ANGLE ),
    rotate_z( TEST_ANGLE ),
    NULL,
  };
  matrix4x4_t *r, *t;

  str = matrix4x4_str( *pipeline[ 0 ] );
  str2 = matrix4x4_str( *pipeline[ 1 ] );
  str3 = matrix4x4_str( *pipeline[ 2 ] );

  info( "pipeline =\n%s\n%s\n%s\n", str, str2, str3 );

  free( str );
  free( str2 );
  free( str3 );

  t = matrix4x4_multiply( *pipeline[ 0 ], *pipeline[ 1 ] );
  r = matrix4x4_multiply( *t, *pipeline[ 2 ] );
  m = compile_pipeline( pipeline );
  free_pipeline_matrices( pipeline );

  str = matrix4x4_str( *m );
  str2 = matrix4x4_str( *r );
  str3 = matrix4x4_str( *t );

  info( "parcial =\n%s\n", str3 );
  info( "final =\n%s\n", str2 );
  info( "compiled pipeline =\n%s\n", str );

  free( str );
  free( str2 );
  free( str3 );


  assert_equal_double( m->data[ 0 ][ 0 ], r->data[ 0 ][ 0 ], DOUBLE_PRECISION);
  assert_equal_double( m->data[ 0 ][ 1 ], r->data[ 0 ][ 1 ], DOUBLE_PRECISION);
  assert_equal_double( m->data[ 0 ][ 2 ], r->data[ 0 ][ 2 ], DOUBLE_PRECISION);
  assert_equal_double( m->data[ 0 ][ 3 ], r->data[ 0 ][ 3 ], DOUBLE_PRECISION);
  assert_equal_double( m->data[ 1 ][ 0 ], r->data[ 1 ][ 0 ], DOUBLE_PRECISION);
  assert_equal_double( m->data[ 1 ][ 1 ], r->data[ 1 ][ 1 ], DOUBLE_PRECISION);
  assert_equal_double( m->data[ 1 ][ 2 ], r->data[ 1 ][ 2 ], DOUBLE_PRECISION);
  assert_equal_double( m->data[ 1 ][ 3 ], r->data[ 1 ][ 3 ], DOUBLE_PRECISION);
  assert_equal_double( m->data[ 2 ][ 0 ], r->data[ 2 ][ 0 ], DOUBLE_PRECISION);
  assert_equal_double( m->data[ 2 ][ 1 ], r->data[ 2 ][ 1 ], DOUBLE_PRECISION);
  assert_equal_double( m->data[ 2 ][ 2 ], r->data[ 2 ][ 2 ], DOUBLE_PRECISION);
  assert_equal_double( m->data[ 2 ][ 3 ], r->data[ 2 ][ 3 ], DOUBLE_PRECISION);
  assert_equal_double( m->data[ 3 ][ 0 ], r->data[ 3 ][ 0 ], DOUBLE_PRECISION);
  assert_equal_double( m->data[ 3 ][ 1 ], r->data[ 3 ][ 1 ], DOUBLE_PRECISION);
  assert_equal_double( m->data[ 3 ][ 2 ], r->data[ 3 ][ 2 ], DOUBLE_PRECISION);
  assert_equal_double( m->data[ 3 ][ 3 ], r->data[ 3 ][ 3 ], DOUBLE_PRECISION);

  matrix4x4_free( t );
  matrix4x4_free( r );
  matrix4x4_free( m );

  return 0;
}
